#!/usr/bin/python
# coding=utf-8
import rospy
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from lejulib import *
import numpy as np
from motion.motionControl import *
import cv2


"""话题信息"""
topic_raw = "/camera/color/image_raw"
topic_depth = "/camera/depth/image_rect_raw"
topic_chin="/chin_camera/image"

hsv=[
    (np.array([0,0,132]),np.array([179,70,255])),  
    (np.array([122,159,98]),np.array([179,255,216])), 
    (np.array([28,169,85]),np.array([149,255,170])), 
    (np.array([0,152,143]),np.array([179,255,255])), 
]

ANGLE=1
DEMO=1
WHITE=0
RED=1
BLUE=2
YELLOW=3

GAIT_RANGE = 0.05 
ROTATION_RANGE = 10.0 
ROI = (100, 100) 

def white_line(image):
    frame = bridge.imgmsg_to_cv2(image, 'bgr8')  
    hsv_image = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

    kernel_4 = np.ones((4, 4), np.uint8)

    mask = cv2.inRange(hsv_image, hsv[WHITE][0],hsv[WHITE][1])


    target = cv2.bitwise_and(frame, frame, mask=mask)
    ret, binary = cv2.threshold(mask, 127, 255, cv2.THRESH_BINARY)
    _,contours, hierarchy = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    height = frame.shape[0]
    width = frame.shape[1]
    if len(contours) > 0:
        x,y,w,h=cv2.boundingRect(contours[-1])
        if w*h> 20000:
            return True
        else :
            return False
    else:
        return False

def blue_target(image,target):
    frame = bridge.imgmsg_to_cv2(image, 'bgr8')  
    hsv_image = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)  

    kernel_4=np.ones((4,4),np.uint8)
    mask=cv2.inRange(hsv_image,hsv[target][0],hsv[target][1])
    erosion=cv2.erode(mask,kernel_4,iterations=1)
    erosion=cv2.erode(erosion,kernel_4,iterations=1)
    dilation=cv2.dilate(erosion,kernel_4,iterations=1)
    dilation=cv2.dilate(dilation,kernel_4,iterations=1)
    
    target=cv2.bitwise_and(frame,frame,mask=dilation)
    ret,binary=cv2.threshold(dilation,127,255,cv2.THRESH_BINARY)
    _,contours,hierarchy=cv2.findContours(binary,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
    height = frame.shape[0]
    width = frame.shape[1]
    if len(contours) >0:
        x,y,w,h=cv2.boundingRect(contours[-1])
        if (w*h)>10000:
            return True
        else :return False
        


def walk_target(frame,DEMO): 
    hsv_image = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)  
    kernel_2 = np.ones((2, 2), np.uint8)
    kernel_3 = np.ones((3, 3), np.uint8)
    kernel_4 = np.ones((4, 4), np.uint8)

    mask = cv2.inRange(hsv_image,hsv[DEMO][0],hsv[DEMO][1])

    target = cv2.bitwise_and(frame, frame, mask=mask)
    ret, binary = cv2.threshold(mask, 127, 255, cv2.THRESH_BINARY)
    _,contours, hierarchy = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    height = frame.shape[0]
    width = frame.shape[1]
    if len(contours) > 0:
        x, y, w, h = cv2.boundingRect(contours[-1])
        cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255), 3)
        center_x = x + w // 2
        center_y = y + h // 2
    
        if center_x > (height // 2 + 100):
            return "right"
        elif center_x < (height // 2 - 100):
            return "left"
        elif (height // 2 - 100) < center_x <(height // 2 + 100) :
            return "walk"
    else:
        return "left"


def slow_walk(direction, stepnum=1, angle=None):
    array = [0.0, 0.0, 0.0]
    if direction == "forward":
        array[0] = GAIT_RANGE
    elif direction == "backward":
        array[0] = -1 * GAIT_RANGE
    elif direction == "rotation":
        array[2] = ROTATION_RANGE if angle > 0 else -1 * ROTATION_RANGE
        stepnum = int(abs(angle) / ROTATION_RANGE)
    else:
        rospy.logerr("error walk direction")
    for _ in range(stepnum):
        SendGaitCommand(array[0], array[1], array[2])


def move(mean_distance,DEMO):
    if(DEMO%2!=0):
        ANGLE=1
    else:
        ANGLE=-1
    print(mean_distance)
    if mean_distance < 150:
        slow_walk("backward", 1)
    elif mean_distance > 1000:
        slow_walk("forward", 2)
        WaitForWalkingDone()
        direction=find_color(rospy.wait_for_message(topic_raw, Image),DEMO)
        if direction=="left":
            slow_walk("rotation", angle=15)
            WaitForWalkingDone()
        elif direction =="right":
            slow_walk("rotation", angle=-15)
            WaitForWalkingDone()

    elif mean_distance > 600:
        slow_walk("forward", 2)
        WaitForWalkingDone()
        direction=find_color(rospy.wait_for_message(topic_raw, Image),DEMO)
        if direction=="left":
            slow_walk("rotation", angle=15)
            WaitForWalkingDone()
        elif direction =="right":
            slow_walk("rotation", angle=-15)
            WaitForWalkingDone()
    elif mean_distance < 250:
        slow_walk("backward", 1)
    else:
        angle=10
        while angle<90:
            slow_walk("rotation",angle=10*ANGLE)
            angle=angle+10
        WaitForWalkingDone()
        while not white_line(rospy.wait_for_message(topic_chin,Image)):
            slow_walk("forward",1)
        WaitForWalkingDone()
        angle=10
        while angle<60:
            slow_walk("rotation",angle=-10*ANGLE)
            angle=angle+10
        WaitForWalkingDone()
        if DEMO+1>3:
            DEMO=2
        while not blue_target(rospy.wait_for_message(topic_raw,Image),DEMO+1):
            slow_walk("rotation",angle=-15*ANGLE)
            WaitForWalkingDone()
        DEMO=DEMO+1
def find_color(image,DEMO):
    cv_color = bridge.imgmsg_to_cv2(image, 'bgr8')  
    return walk_target(cv_color,DEMO)

def callback(image):
    cv_image = bridge.imgmsg_to_cv2(image, "16UC1")  

    cv_image = np.array(cv_image)  
    height, width = cv_image.shape  
    roi_image = cv_image[height / 2 - ROI[1] / 2: height / 2 + ROI[1] / 2,
                width / 2 - ROI[0] / 2: width / 2 + ROI[0] / 2]  
    mean_distance = roi_image.mean()  
    return mean_distance  


if __name__ == "__main__":
    rospy.init_node('roban_avoidance')
    rospy.Subscriber('terminate_current_process', String, terminate) 
    bridge = CvBridge() 
    cv_color=np.zeros((640,480))

    print SetBodyhubTo_walking(2)
    while not rospy.is_shutdown():
        means_distance = []
        for _ in range(5):
            means_distance.append(
                callback(rospy.wait_for_message(topic_depth, Image)))
        move(sum(means_distance) / len(means_distance),DEMO)